#include "zf_common_headfile.h"

#define MY_SERVO_MOTOR_DUTY(x)     ((float)10000/(1000.0/(float)300)*(0.5+(float)(x)/90.0))
extern int Left_Encoder;
extern int Right_Encoder;
extern int CAR_STOP_St;//强制关闭舵机标志位
int Speed_Target=600;//目标速度
int Min_Speed_Target=400;//目标速度
int Sebd_speed=0;
int Left_OUT=0;
int Right_OUT=0;
float Servo_Agle=0;
int Last_Car_RUN=2;
float Sensitivity = 0;
float Last_Sensitivity = 0;
float Temp_Sensitivity = 0;
float Speed_Min_Proportion=0;
float Stretch_Coefficient=0.4;
float Speed_Proportion=0;
float Speed_Proportion_Array[100] = {0};    //用于滑动均值滤波

// //无用速度
// int Compare_Speed_Max_Big=Speed_Target+50;
// int Compare_Speed_Max_Smal=Speed_Target;

//设置自动控制速度的最大值和最小值
int Max_Speed=Speed_Target;//自动控制的最大速度、、非特殊元素内
int Min_Speed=Min_Speed_Target;//自动控制最小速度

//直道速度
int ZhiDao_Speed=Max_Speed;//直道速度，通常为自动控制最大速度
//
int ChongCi_Speed=ZhiDao_Speed+50;//直道冲刺速度

//障碍物最大速度
int BiZhang_Max_Speed=Max_Speed;//障碍无物最大速度
int BiZhang_MinSpeed=BiZhang_Max_Speed;//障碍物最小速度，通常大于自动控制最小速度

//右环岛速度
int Right_Huandao_Max_Speed=Max_Speed;//右环岛最大速度
int Right_Huandao_Min_Speed=Right_Huandao_Max_Speed-50;//右环岛最小速度，可以小于自动控制最小速度
//左环岛速度
int Left_Huandao_Max_Speed=Max_Speed;//左环岛最大速度
int Left_Huandao_Min_Speed=Left_Huandao_Max_Speed-50;//左环岛最小速度，可以小于自动控制最小速度

//坡道速度
int Podao_Max_Speed=300;//可以小于自动控制最小速度
int Podao_Min_Speed=Podao_Max_Speed-50;//可以小于自动控制最小速度

//弯道速度十字速度
int WanDao_Max_Speed=Max_Speed-40;//不可以小于自动控制最小速度
int WanDao_Min_Speed=Min_Speed;//不可以小于自动控制最小速度

float Speed_Value[7] = {0.0f, 0.166f, 0.333f, 0.5f, 0.666f, 0.833f, 1.0f};  //速度采用了归一化，将1.0均分为六段

float Speed_Vague_Array[4][4] = { {0, 1, 2, 3},
                                  {1, 2, 3, 4},
                                  {3, 4, 5, 6},
                                  {5, 6, 6, 6}};    //映射数组
 
int16 Y_Meet_Count = 0;     //累积满足相遇点条件的次数，比如由弯道进入直道时，车身未转正时Y_Meet已经趋近于2了，此时加速会导致车轨迹不稳
                            //那我们就进行一定的缓冲，当Y_Meet满足我们设定的条件一定次数后，再去执行相应的加速程序，可确保车身转正再加速
int Stop_Moter_Flag=0;
int Speed_Angle[20];
int speed_Sum=0;
int Spedd=0;
int CAR_RUN2=0;
//电机速度控制
void Contorl()
{
    Get_Motor_PID();
    if(CAR_RUN==1)
    {
        Sebd_speed=Speed_Proportion;
        //坡道减速
        if(PoDao_Flag!=0)
        {
            Sebd_speed=300;
        }
        //环岛减速
        if(HuanDao_Stas!=0 && Speed_Target>=550)
        {
            Sebd_speed=500;
        }
    }
    else if (CAR_RUN==0)
    {
        Sebd_speed=0;
    }
    if(CAR_RUN==1)
    {
        // Right_OUT=(int)Moter_Right_PID(Sebd_speed,((Right_Encoder+Left_Encoder)/2)); 
        CAR_RUN2=1;
    }
    else if(CAR_RUN==0)
    {
        // Spedd++;
    }
    if(CAR_RUN2==1)
    {
        Right_OUT=(int)Moter_Right_PID(Sebd_speed,((Right_Encoder+Left_Encoder)/2)); 
    }
    else if(Spedd>200)
    {
        CAR_RUN2=0;
    }
    /***********************************输出************************************************ */
    //电机输出
    if(Right_OUT>=0)
    {
        gpio_set_level(MOTOR1_DIR, 0);
        pwm_set_duty(MOTOR1_PWM, Right_OUT);
    }
    else if(Right_OUT<0)
    {
        gpio_set_level(MOTOR1_DIR, 1);
        pwm_set_duty(MOTOR1_PWM, -Right_OUT);
    }
    if(Right_OUT>=0)
    {
        gpio_set_level(MOTOR2_DIR, 0);
        pwm_set_duty(MOTOR2_PWM, Right_OUT);
    }
    else if(Right_OUT<0)
    {
        gpio_set_level(MOTOR2_DIR, 1);
        pwm_set_duty(MOTOR2_PWM, -Right_OUT);
    }
    if(UDP_Flag==1 && CAR_RUN==1)
    {
        // seekfree_assistant_oscilloscope_data.data[0] = ((Right_Encoder+Left_Encoder)/2);
        // seekfree_assistant_oscilloscope_data.data[1] = Sebd_speed;
        // seekfree_assistant_oscilloscope_data.channel_num = 2;
        // seekfree_assistant_oscilloscope_send(&seekfree_assistant_oscilloscope_data);
        // seekfree_assistant_data_analysis();
        // seekfree_assistant_oscilloscope_data.data[0] = Mid_Error;
        // seekfree_assistant_oscilloscope_data.data[1] = 0;
        // seekfree_assistant_oscilloscope_data.channel_num = 2;
        // seekfree_assistant_oscilloscope_send(&seekfree_assistant_oscilloscope_data);
        // seekfree_assistant_data_analysis();
    }
}
int Last_Sevor_CAR_STOP=2;
int Servo_Image_Flag=0;
int Servo_Image=0;
//舵机角度控制
void Servo_Control()
{
    if(CAR_RUN==1)
    {
        //计算舵机角度
        Servo_Agle=Fuzz_PID(Mid_Error,0);
        //舵机角度输出
        pwm_set_duty(SERVO_MOTOR1_PWM, MY_SERVO_MOTOR_DUTY(Servo_Agle));
    }
    //延时关闭舵机转向控制
    if(CAR_RUN==0 && Last_Sevor_CAR_STOP==1 && CAR_STOP_St!=1)
    {
        Servo_Image_Flag=1;
    }
    if(Servo_Image_Flag==1)
    {
        Servo_Image++;
        Servo_Agle=Fuzz_PID(Mid_Error,0);
        //舵机角度输出
        pwm_set_duty(SERVO_MOTOR1_PWM, MY_SERVO_MOTOR_DUTY(Servo_Agle));
        if(Servo_Image>=10)
        {
            Servo_Image=0;
            Servo_Image_Flag=0;
        }
    }
    Last_Sevor_CAR_STOP=CAR_RUN;
}
/*********************************串级差速*********************************/
float dirn_p=0,dirn_d=0,dir_p=0,dir_d=0;
float gyroz1=0;
//差速串级PID
int dircontrol(int chazhi)//外环
{
	int out_dw;
	static int last_chazhi;
	out_dw=(int)(chazhi*dir_p+(chazhi-last_chazhi)*dir_d);
	last_chazhi=chazhi;
    return out_dw;
}
int diranglecontrol(int out_dw)//内环
{
	int out_dn,error;
	static int last_error;
	error=(out_dw-gyroz1);//内环的偏差是外环输出与陀螺仪z轴的角速度之差
	out_dn=(int)(error*dirn_p+(error-last_error)*dirn_d);
	last_error=error;
	return out_dn;
}
/******************************速度决策********************************/

/**
 * 函数功能：      限制浮点数在指定范围内
 * 形  参：        float value     输入值
 *                 float min_val   最小值
 *                 float max_val   最大值
 * 返回值：        限幅后的值
 */
 float Limit_Float(float value, float min_val, float max_val)
 {
     if(value < min_val) {
         return min_val;
     }
     else if(value > max_val) {
         return max_val;
     }
     else {
         return value;
     }
 }
float Last_Curvature_Value = 0;     //防止
/**
* 函数功能：      计算中线曲率，作为控制速度的一个数据
* 特殊说明：      无
* 形  参：        uint8 *line      中线
*                 uint8 start      爬线相遇点的Y值，或最大有效行
*                 uint8 end        爬线起始行（或图像最底端）
*
* 示例：          Calculate_Curvature_2(C_Line, Y_Meet, L_Start_Point[1]);
* 返回值：        Last_Curvature_Value      所得曲率
*/
float Calculate_Curvature_2(uint8 *line, uint8 start, uint8 end)
{
    int16 Sum_Difference_Value = 0;     //用于X坐标差值的累积
    int16 Sum_Weight = 0;               //用于权值相加
    uint8 i = 0;
 
    uint8 Base_Curvature_Weight[80] = {
    0,0,0,0,0,0,0,0,0,0,//0~9
    0,0,0,0,0,0,0,0,0,0,//10~19
    0,0,0,0,0,0,0,0,0,0,//20~29
    79,77,74,72,70,68,66,64,62,60,//30~39
    58,56,54,52,50,48,46,43,40,38,//40~49
    1,1,1,1,1,1,1,1,1,1,//50~59
    1,1,1,1,1,1,1,1,1,1,//60~69
    0,0,0,0,0,0,0,0,0,0,//70~79
                                                    };//误差权重      //固定权值，0,1,0,1间隔加权
    // uint8_t Trends_Curvature_Weight[11] =  {9,9,9,10,10,10,10,10,10,10,10};
 
    // uint8_t Start_Line = (uint8_t)(hightest_Average / 35.0f * 6.0f  + 40.0f);//39
 
    // // //将动态权值赋值给固定权值数组
    // for(i = 0; i < 11; i ++)
    // {
    //     Base_Curvature_Weight[Start_Line - i] = Trends_Curvature_Weight[i];
    // }
 
    for(i = start; i < end - 1; i++)    //求差值和并求出权值
    {
        Sum_Difference_Value += (int16)(abs((int)line[i] - (int)line[i + 1]) * (int)Base_Curvature_Weight[i]);
        Sum_Weight += (int16)Base_Curvature_Weight[i];
    }
 
    if(Sum_Weight != 0)     //防止权值为0，但好像不会出现，当时不知道为啥要写这个
    {
        Last_Curvature_Value = (float)Sum_Difference_Value / (float)Sum_Weight;     //求加权平均值，归一化放在了另一个函数里
        return Last_Curvature_Value;
    }
    else
    {
        return Last_Curvature_Value;//曲率
    }
}
/**
* 函数功能：      使用相遇点和曲率映射速度，参照模糊PID思想
* 特殊说明：      无
* 形  参：        float sensitivity                //曲率
*                 float speed_min_proportion       //速度最小比率（设定最小速度与最大速度的比值），为0~1之间
*                 float Speed_max_proportion       //速度最大比率，通常为1
*                 uint8 y_meet                     //爬线相遇点的Y坐标
*                 uint8 min_y_meet                 //爬线相遇点的最小值，看得越远值越小
*                 uint8 max_y_meet                 //爬线相遇点的最大值，看得越远值越大
*
* 示例：          Calculate_Curvature_2(C_Line, Y_Meet, L_Start_Point[1]);
* 返回值：        Last_Curvature_Value      所得曲率
*/
float Speed_Mapping(float sensitivity, float speed_min_proportion, float Speed_max_proportion, uint8 y_meet, uint8 min_y_meet, uint8 max_y_meet)
{
    // uint16 i = 0;
    float VH = 0, VE = 0;
    float X2Y = 0;
    float X1Y = 0;
    float Y2X = 0;
    float Y1X = 0;
    uint8 View_Effictive_Line = max_y_meet - min_y_meet;
    if(ShiZi_Flag2==1 ||  WanDao_Flag==1)
    {
        Y_Meet_Count = 0;
        Max_Speed = WanDao_Max_Speed;//设置弯道最大速度
    }
    //另一方面更重要的是为了防止误判，导致乱加减速
    //冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！
    //冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！
    //冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！
    else if(y_meet <= 4 && Max_Speed == ZhiDao_Speed)
    {
        Y_Meet_Count ++;
        if(Y_Meet_Count >= 30)
        {
            Y_Meet_Count = 0;
            Max_Speed = ChongCi_Speed;//设置冲刺速度
        }
    }
    //一次不满足就重新来，比较严格，可以不用或者判定放缓一些
    else if(y_meet >= 4 && Max_Speed == ZhiDao_Speed)
    {
        Max_Speed = ZhiDao_Speed;//二次判断冲刺速度
        Y_Meet_Count = 0;
    }
    //直道到弯道的缓冲（作用于弯道在图像最远端，还未识别到弯道时），这个缓冲时间短一些，减速快些，这两个计数值根据自己情况调整
    else if(y_meet >= 4 && Max_Speed == ChongCi_Speed)//用于直道加速提前减会弯道速度
    {
        Y_Meet_Count --;
        if(Y_Meet_Count <= -5)
        {
            Y_Meet_Count = 0;
            Max_Speed = ZhiDao_Speed;//冲刺速度返回直道速度
        }
    }
    //环岛速度控制
    if (HuanDao_Stas!=0 && HuanDao_Stas>=8)//右环岛速度设置
    {
        Y_Meet_Count = 0;
        Min_Speed=Right_Huandao_Min_Speed;//设置环岛最小速度
        Max_Speed = Right_Huandao_Max_Speed;//设置环岛最大速度
    }
    else if (HuanDao_Stas!=0 && HuanDao_Stas<=7)
    {
        Y_Meet_Count = 0;
        Min_Speed=Left_Huandao_Min_Speed;//设置环岛最小速度
        Max_Speed = Left_Huandao_Max_Speed;//设置环岛最大速度
    }//环岛速度恢复直道速度
    else if (HuanDao_Stas==0 &&  Max_Speed == Right_Huandao_Max_Speed)
    {
        Y_Meet_Count = 0;
        Max_Speed = ZhiDao_Speed;//恢复直道最大速度
        Min_Speed=Min_Speed_Target;//恢复最小速度
    }
    else if (HuanDao_Stas==0 &&  Max_Speed == Left_Huandao_Max_Speed)
    {
        Y_Meet_Count = 0;
        Max_Speed = ZhiDao_Speed;//恢复直道最大速度
        Min_Speed=Min_Speed_Target;//恢复最小速度
    }
    //坡道将速度拉到最低
    if(PoDao_Flag==1)
    {
        Min_Speed=Podao_Min_Speed;//将坡道速度减到最小
        return Podao_Max_Speed;//直接返回坡道速度
    }
    else if (PoDao_Flag==0 &&  Min_Speed==Podao_Min_Speed)
    {
        Min_Speed=Min_Speed_Target;//恢复直道最小速度
    }
    //当识别出元素是直道，爬线相遇点位于顶端，左右两侧几乎没有落在边框上的点，并且中线曲率很小时，直接返回最大速度开冲！
    //此处判定十分严格，不会误判，不满足时才执行下方速度拟合部分代码
    if(ZhiDao_Flag == 1 && y_meet <= 4 && L_Border_Point_Num <= 5 && R_Border_Point_Num <= 5 && sensitivity <= 0.9f)
    {
        return Max_Speed;//直接返回直线最大速度或者1冲刺速度
    }
    //避障减速
    if(BiZhang_Flag==1 || BiZhang_Flag==2)
    {
        return BiZhang_Max_Speed;//设置避障最小速度
    }
    //下面使用曲率和爬线相遇点Y坐标拟合速度
    //与模糊PID原理相同，不过多注释
    VH = ((float)(y_meet - 2) * 3.0f / (float)View_Effictive_Line);
    VE = sensitivity * 3.0f;
 
    int8 VH1 = (int8)VH;
    if (VH1 > VH)
    {
      VH--;
    }
    int8 VH2 = VH1 + 1;
 
    int8 VE1 = (int8)VE;
    if (VE1 > VE)
    {
      VE1--;
    }
    int8 VE2 = VE1 + 1;
 
    if (VH1 > 3)
    {
      VH1 = 3;
    }
 
    if (VH2 > 3)
    {
      VH2 = 3;
    }
 
    if (VE1 > 3)
    {
      VE1 = 3;
    }
 
    if (VE2 > 3)
    {
      VE2 = 3;
    }
 
    X2Y = (Speed_Vague_Array[VH1][VE2] - Speed_Vague_Array[VH1][VE1]) * (VE - VE1) + Speed_Vague_Array[VH1][VE1];
 
    X1Y = (Speed_Vague_Array[VH2][VE2] - Speed_Vague_Array[VH2][VE1]) * (VE - VE1) + Speed_Vague_Array[VH2][VE1];
 
    Y2X = (Speed_Vague_Array[VH2][VE1] - Speed_Vague_Array[VH1][VE1]) * (VH - VH1) + Speed_Vague_Array[VH1][VE1];
 
    Y1X = (Speed_Vague_Array[VH2][VE2] - Speed_Vague_Array[VH1][VE2]) * (VH - VH1) + Speed_Vague_Array[VH1][VE2];
 
    float Speed_approximation = (X2Y + X1Y + Y2X + Y1X) / 4.0;
 
    int8 Speed_1 = (int8)Speed_approximation;
    if (Speed_1 > Speed_approximation)
    {
        Speed_1--;
    }
    int8 Speed_2 = Speed_1 + 1;
 
    return (1.0f - ((Speed_Value[Speed_2] - Speed_Value[Speed_1]) * (Speed_approximation - Speed_1) + Speed_Value[Speed_1])) * (float)(Max_Speed - Min_Speed) + (float)Min_Speed;   //返回拟合好的速度
}
/**
* 函数功能：      进行速度控制使用相遇点和曲率映射速度，参照模糊PID思想
* 特殊说明：      无
* 形  参：        uint8 Auto_Control_Flag           是否要车辆自行控制速度
*                                                   //当自动控制标志位为1时，速度根据中线曲率和相遇点进行拟合
*                                                   //当自动控制标志位为0时，速度由手动输入                              
*                 uint16 Manual_Control_Value       //手动控制速度
*
* 示例：          Fitting_Speed_Control(1, 0);
* 返回值：        无
*/
void Fitting_Speed_Control(uint8 Auto_Control_Flag, uint16 Manual_Control_Value, uint8 element, uint8 element_state, uint8 start, uint8 end)
{
    uint8 i = 0;
    if(Auto_Control_Flag == 1)
    {
        float Temp_Speed_Proportion = 0;
        //计算中线曲率，用于速度控制
        Last_Sensitivity = Sensitivity;
        Temp_Sensitivity = Calculate_Curvature_2(center_line,hightest,start_point_l[1])/ 1.0f;
 
        //计算最小速度比率
        Speed_Min_Proportion = (float)Min_Speed / (float)Max_Speed;
 
        //Stretch_Coefficient：减速拉伸系数，此参数越大，减速距离越短，最大值为1
        //Stretch_Coefficient 等于手动设定一个曲率最小阈值，小于这个阈值时直接将曲率设为0
        if(Temp_Sensitivity <= Stretch_Coefficient)
        {
            Sensitivity = 0.0f;
        }
        else    //将（阈值~1.0）之间的数进行拉伸，可以代入几个实际的数理解下这部分原理
        {
            float Temp_Float_Num = (Temp_Sensitivity - Stretch_Coefficient) * (1.0f / (1.0f - Stretch_Coefficient));
            Sensitivity = Limit_Float(Temp_Float_Num * 0.3f + Last_Sensitivity * 0.7f, 0.01f, 1.0f);    //曲率值滤波
        }
 
        Temp_Speed_Proportion = Speed_Mapping(Sensitivity, Speed_Min_Proportion, 1.0f, hightest, 1, 35);  //计算速度
        Speed_Proportion = Limit_Float(Temp_Speed_Proportion, (float)Min_Speed, (float)Max_Speed);  //速度限幅
 
        //滑动均值滤波，防止速度突变
        float Speed_Proportion_Sum = 0;
        for(i = 0; i < 99; i ++)
        {
            Speed_Proportion_Array[i + 1] = Speed_Proportion_Array[i];
        }
        Speed_Proportion_Array[0] = Speed_Proportion;
        for(i = 0; i < 100; i ++)
        {
            Speed_Proportion_Sum += Speed_Proportion_Array[i];
        }
        Speed_Proportion = Speed_Proportion_Sum / 100.0f;
    }    
    else
    {
        //手动速度靠按键实现调整
        Speed_Proportion=Speed_Target;
        //手动控制速度部分
    }
}
